#!/usr/bin/env python
PACKAGE = "avoidance"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# local_planner

gen.add("box_radius_",    double_t,    0, "Data points farther away will be discarded", 12.0,  0, 20)
gen.add("goal_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 10.0, 0, 20.0)
gen.add("heading_cost_param_", double_t, 0, "Cost function weight for constant heading", 0.5, 0, 20.0)
gen.add("smooth_cost_param_", double_t, 0, "Cost function weight for path smoothness", 1.5, 0.5, 20.0)
gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0)
gen.add("no_progress_slope_", double_t, 0, "If progress derivative higher than this value the drone rises", -0.0007, -1.0, 1.0)
gen.add("min_realsense_dist_", double_t, 0, "Discard points closer than that", 0.2, 0, 10)
gen.add("timeout_startup_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 5, 0, 60)
gen.add("timeout_critical_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 0.5, 0, 10)
gen.add("timeout_termination_", double_t, 0, "After this timeout the companion status is MAV_STATE_FLIGHT_TERMINATION", 15, 0, 1000)
gen.add("max_point_age_s_", double_t, 0, "maximum age of a remembered data point", 20, 0, 500)
gen.add("min_num_points_per_cell_", int_t, 0, "minimum number of points in one area to be kept, if lower they are discarded as noise", 1, 1, 500)
gen.add("smoothing_speed_xy_", double_t, 0, "response speed of the smoothing system in xy (set to 0 to disable)", 10, 0, 30)
gen.add("smoothing_speed_z_", double_t, 0, "response speed of the smoothing system in z (set to 0 to disable)", 3, 0, 30)
gen.add("smoothing_margin_degrees_", double_t, 0, "smoothing radius for obstacle cost in cost histogram", 40, 0, 90)

gen.add("use_vel_setpoints_", bool_t, 0, "Enable velocity setpoints (if false, position setpoints are used)", False)
gen.add("adapt_cost_params_", bool_t, 0, "If no progress towards goal is made, allow rising", True)

# star_planner
gen.add("children_per_node_",    int_t,    0, "Branching factor of the search tree", 50,  0, 100)
gen.add("n_expanded_nodes_",    int_t,    0, "Number of nodes expanded in complete tree", 10,  0, 200)
gen.add("tree_node_distance_",    double_t,    0, "Distance between nodes", 1,  0, 20)
gen.add("tree_discount_factor_",    double_t,    0, "Discount factor in tree cost function", 0.8,  0, 1)
gen.add("max_path_length_",    double_t,    0, "Maximum length of planned paths", 3,  0, 15)

exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode"))
